


#ifndef MATRIX4_H
#define MATRIX4_H


#include <Vec4.h>


class Matrix4
{

	/* elements indexed as [row][col]  == row + (4 * col), i.e. 
	 
	 00 01 02 03  ->  0  4  8  12 
	 10 11 12 13  ->  1  5  9  13
	 20 21 22 23  ->  2  6  10 14
	 30 31 32 33  ->  3  7  11 15
	 
	 */
	
public:
		
	//initialization
	Matrix4()
	{
		setIdentity();
	}

	Matrix4(const float* m)
	{
		std::memcpy( m_matrix, m, 16*sizeof(float) );
	}

	Matrix4(const Matrix4& other)
	{
		std::memcpy( m_matrix, other.m_matrix, 16*sizeof(float) );
	}

	inline void setIdentity()
	{
		std::memset( m_matrix, 0, 16*sizeof(float) );
		m_matrix[0] = 1.0f;
		m_matrix[5] = 1.0f;
		m_matrix[10] = 1.0f;
		m_matrix[15] = 1.0f;
	}
	
	// assignment
	Matrix4& operator=(const Matrix4& rhs)
	{
			std::memcpy( m_matrix, rhs.m_matrix, 16*sizeof(float) );
			return *this;
	}

	// invert in place, return true on success, false on failure
	bool invert();
	
	// Sets the translation column to the given vector
	void setTranslation(const Vec4& translation);
	Vec4 getTranslation() const;
	
	// Sets the 3x3 rotation part of the matrix to the rotaton with given axis and angle (in radians)
	void setRotation( const Vec4& axis, float angle );
	
	// Sets the 3x3 rotation part of the matrix to the rotaton which rotates the given unit "from" vector into the given unit "to" vector
	void setRotation( const Vec4& from, const Vec4& to );
	
	// ALGEBRA
	
	//element access
	float& operator[](const size_t index);
	const float& operator[](const size_t index) const;
	
	// return transpose
	Matrix4 getTranspose();

	// transpose in-place
	void tranpose();

	// addition and subtraction
	friend Matrix4& operator+=( Matrix4& lhs, const Matrix4& rhs );
	friend Matrix4 operator+( const Matrix4& lhs, const Matrix4& rhs );
	
	friend Matrix4& operator-=( Matrix4& lhs, const Matrix4& rhs );
	friend Matrix4 operator-( const Matrix4& lhs, const Matrix4& rhs );
	
	// multiplication
	friend Matrix4& operator*=( Matrix4& lhs, const Matrix4& rhs );
	friend Matrix4 operator*( const Matrix4& lhs, const Matrix4& rhs );
	
	// right multiply a Matrix4 by a homogeneous Vec4 
	friend Vec4 operator*( const Matrix4& lhs, const Vec4& rhs );
	
	
public:	
	
	float m_matrix[16];
		
};


inline float& Matrix4::operator[](const size_t index)
{
	return m_matrix[index];
}

inline const float& Matrix4::operator[](const size_t index) const
{
	return m_matrix[index];
}

inline Matrix4 Matrix4::getTranspose()
{
	Matrix4 Tr;
	for (int i=0; i<4; ++i)
	{
		int i4 = 4*i;
		for (int j=0; j<4; ++j)
		{
			Tr[i+4*j] = m_matrix[j+i4];
		}
	}
	return Tr;
}

inline void Matrix4::tranpose()
{
	Matrix4 Tr = getTranspose();
	std::memcpy( m_matrix, Tr.m_matrix, 16*sizeof(float) );
}

inline Matrix4& operator+=( Matrix4& lhs, const Matrix4& rhs )
{
	for (int i=0; i<16; ++i)
	{
		lhs.m_matrix[i] += rhs.m_matrix[i];
	}
	return lhs;
}

inline Matrix4& operator-=( Matrix4& lhs, const Matrix4& rhs )
{
	for (int i=0; i<16; ++i)
	{
		lhs.m_matrix[i] -= rhs.m_matrix[i];
	}
	return lhs;
}

inline Matrix4 operator+( const Matrix4& lhs, const Matrix4& rhs )
{
	Matrix4 ret(lhs);
	ret += rhs;
	return ret;
}

inline Matrix4 operator-( const Matrix4& lhs, const Matrix4& rhs )
{
	Matrix4 ret(lhs);
	ret -= rhs;
	return ret;
}

inline Matrix4& operator*=( Matrix4& lhs, const Matrix4& rhs )
{
	Matrix4 mul;
	for (int i=0; i<4; ++i)
	for (int j=0; j<4; ++j)
	{
		int j4 = 4*j;
		int ij4 = i + j4;
		mul.m_matrix[ij4] = 0.0f;
		
		for (int k=0; k<4; ++k)
		{
			mul.m_matrix[ij4] += lhs.m_matrix[i+4*k] * rhs.m_matrix[k+j4]; 
		}
	}

	lhs = mul;
	return lhs;
}

inline Matrix4 operator*( const Matrix4& lhs, const Matrix4& rhs )
{
	Matrix4 ret(lhs);
	ret *= rhs;
	return ret;
}

inline Vec4 operator*( const Matrix4& lhs, const Vec4& rhs )
{
	float x, y, z, w;
	x = y = z = w = 0.0f;
	
	for (int j=0; j<4; ++j)
	{			
		int i = 4*j;
		float v = rhs[j];
		x += lhs.m_matrix[i++] * v;
		y += lhs.m_matrix[i++] * v;
		z += lhs.m_matrix[i++] * v;
		w += lhs.m_matrix[i  ] * v;
	}
	
	Vec4 ret(x, y, z, w);
	return ret;		
}



#endif //MATRIX4_H


